# 内置库
import math
import time
import os
import statistics

# 第三方库
import numpy as np
import matplotlib.pyplot as plt
from shapely.geometry import Point, Polygon
from typing import Dict, List, Tuple, Optional, Union
from hybrid_a_star.hybrid_a_star import hybrid_a_star_planning
from hybrid_a_star.hybrid_a_star import check_car_collision
from hybrid_a_star.hybrid_a_star_noback import hybrid_a_star_planning as hybrid_a_star_noback
# from planner_backup import downsample_image
# from planner_backup import plot_bit_path
import matplotlib.path as mpath


from planner_backup_a2 import process as Planner_backup


dir_algorithm = os.path.dirname(os.path.dirname(__file__))
do_plot = True

def plot_borderline(observation, linewidth=0.5, ax=None):
    if ax is None:
        ax = plt.gca()
    for i in range(len(observation["hdmaps_info"]["tgsc_map"].borderline)):
        borderpoints = observation["hdmaps_info"]["tgsc_map"].borderline[i]['borderpoints']
        x_values = [point[0] for point in borderpoints]
        y_values = [point[1] for point in borderpoints]
        # ax.plot(x_values, y_values, linewidth=linewidth)
        ax.scatter(x_values, y_values, s=linewidth)


def plot_reference_path(observation, linewidth=0.5, ax=None):
    if ax is None:
        ax = plt.gca()
    for i in range(len(observation["hdmaps_info"]["tgsc_map"].reference_path)):
        waypoints = observation["hdmaps_info"]["tgsc_map"].reference_path[i]['waypoints']
        x_values = [point[0] for point in waypoints]
        y_values = [point[1] for point in waypoints]
        ax.plot(x_values, y_values, linestyle='--', linewidth=linewidth)


def plot_goal(goal_x, goal_y, ax=None):
    if ax is None:
        ax = plt.gca()
    polygon = [[goal_x[0], goal_y[0]], [goal_x[1], goal_y[1]], [goal_x[2], goal_y[2]], [goal_x[3], goal_y[3]]]
    ax.fill(*zip(*polygon), color='y', alpha=0.4, label='goal')


def back_cars2_obstacles(x_1, y_1, local_x_range, local_y_range, image_ndarray, dist_1_start, value=False, distance=60):
    distance = dist_1_start if distance > dist_1_start else distance
    x_bit = (x_1 - local_x_range[0]) * 10
    y_bit = (y_1 - local_y_range[0]) * 10
    # 创建与image_ndarray相同形状的网格
    height, width = image_ndarray.shape
    y_indices, x_indices = np.meshgrid(np.arange(height), np.arange(width), indexing='ij')
    # 计算每个像素与给定点的笛卡尔距离
    distances = np.sqrt((y_indices - y_bit) ** 2 + (x_indices - x_bit) ** 2)
    # 找到与给定点距离为distance的所有索引
    indices = np.where(distances <= distance)
    # 创建原始数组的副本
    image_ndarray_copy = np.copy(image_ndarray)
    # 将对应索引位置的像素设置为False
    image_ndarray_copy[indices] = value
    return image_ndarray_copy


def back_cars2_obstacles_1(x_1, y_1, local_x_range, local_y_range, image_ndarray, value=False, distance=10):
    x_bit = (x_1 - local_x_range[0]) * 10
    y_bit = (y_1 - local_y_range[0]) * 10
    # 创建原始数组的副本
    image_ndarray_copy = np.copy(image_ndarray)
    for i in range((int(x_bit-distance)), int(x_bit+distance)):
        for j in range((int(y_bit-distance)), int(y_bit+distance)):
            indices = (int(j),int(i))
            image_ndarray_copy[indices] = value
    # 将对应索引位置的像素设置为False
    return image_ndarray_copy

def start2_obstacles_1(x_1, y_1, local_x_range, local_y_range, image_ndarray, value=True):
    x_bit = (x_1 - local_x_range[0]) * 10
    y_bit = (y_1 - local_y_range[0]) * 10
    # 创建原始数组的副本
    image_ndarray_copy = np.copy(image_ndarray)
    for i in range((int(x_bit)-30), int(x_bit+30)):
        for j in range((int(y_bit)-30), int(y_bit+30)):
            indices = (int(j),int(i))
            image_ndarray_copy[indices] = value
    # 将对应索引位置的像素设置为False
    return image_ndarray_copy



def plot_path_planned(observation, path_planned, start_x, start_y, goal_x, goal_y,
                      astar_path, x_min, x_max, y_min, y_max, traj, spd_planned, scene_name):
    if "tgsc_map" in observation["hdmaps_info"]:
        print(f'存在observation["hdmaps_info"]["tgsc_map"]，绘制道路边界线')
        plot_borderline(observation, linewidth=0.01)
        plot_reference_path(observation, linewidth=0.5)
    plt.scatter(start_x, start_y, marker='o', color='b', s=20, label='start', facecolors='None')
    for i in traj:
        x_1 = traj[i]['0.0']['x']
        y_1 = traj[i]['0.0']['y']
        plt.scatter(x_1, y_1, marker='o', color='y', s=20, label=f'other car {i}', facecolors='None')
    plot_goal(goal_x, goal_y)
    if observation["test_setting"]["scenario_type"] == "loading" and hasattr(astar_path, "xlist"):
        plt.scatter(astar_path.xlist[0], astar_path.ylist[0], marker='o', color='m', s=15,
                    label='Astar start point', facecolors='None')

    if observation["test_setting"]["scenario_type"] == "unloading" and hasattr(astar_path, "xlist"):
        plt.scatter(astar_path.xlist[-1], astar_path.ylist[-1], marker='o', color='m', s=15,
                    label='Astar end point', facecolors='None')
    x_coords = [point[0] for point in path_planned]
    y_coords = [point[1] for point in path_planned]
    plt.scatter(x_coords, y_coords, c=[abs(x) for x in spd_planned], s=0.2, cmap='rainbow')
    plt.xlim(x_min, x_max)
    plt.ylim(y_min, y_max)
    plt.legend(loc='upper right')
    plt.title(scene_name)
    plt.colorbar(label='Speed (m/s)')


def line_equation(x, y, angle):
    if angle != np.pi / 2 and angle != -np.pi / 2:  # 避免斜率为无限大的情况
        m = np.tan(angle)
        b = y - m * x
        return m, b
    else:
        return float('inf'), x  # 对于垂直线，斜率为无限大，返回x作为截距


def calculate_curvature(x1, y1, angle1, x2, y2, angle2):
    # 中点坐标
    # mid_x, mid_y = (x1 + x2) / 2.0, (y1 + y2) / 2.0

    # 如果两个方向角度相同，曲线不是圆弧，是直线
    if angle1 == angle2:
        return 0  # 直线的曲率为0，倒数为无穷大

    # 计算垂直于两点连线的直线的斜率（即中垂线）
    # 避免除以零的错误
    # if x2 != x1:
    #     slope_perpendicular = -(x2 - x1) / (y2 - y1)
    # else:
    #     slope_perpendicular = float('inf')

    # 使用点斜式方程计算两条直线的方程
    # y = mx + b，通过一个点和斜率求b
    # 计算两个方向的直线方程
    m1, b1 = line_equation(x1, y1, angle1)
    m2, b2 = line_equation(x2, y2, angle2)

    # 找到圆心（两条直线的交点）
    if m1 != float('inf') and m2 != float('inf'):
        cx = (b2 - b1) / (m1 - m2)
        cy = m1 * cx + b1
    elif m1 == float('inf'):
        cx = b1
        cy = m2 * cx + b2
    else:
        cx = b2
        cy = m1 * cx + b1

    # 计算圆心到任一点的距离（半径）
    radius = np.sqrt((cx - x1) ** 2 + (cy - y1) ** 2)

    # 计算曲率
    curvature = 1 / (radius + 1e-6)

    return curvature


def distance(point1, point2):
    return math.sqrt((point2[0] - point1[0]) ** 2 + (point2[1] - point1[1]) ** 2)

def is_point_in_polygon(px, py, poly_x, poly_y):
    """
    判断点 (px, py) 是否在由 (poly_x, poly_y) 定义的多边形内部。
    poly_x 和 poly_y 分别是多边形顶点的 x 和 y 坐标列表。
    """
    polygon = [(poly_x[i], poly_y[i]) for i in range(len(poly_x))]
    path = mpath.Path(polygon)
    return path.contains_point((px, py))

class Planner:
    """ego车的轨迹规划器.
    注:业界一般做法是 path planning + speed planning .
    """

    def __init__(self, observation):
        self._goal_x = statistics.mean(observation['test_setting']['goal']['x'])
        self._goal_y = statistics.mean(observation['test_setting']['goal']['y'])
        self.head_key = 'head' if observation['test_setting']['scenario_type'] in ['loading', 'unloading'] else 'heading'
        self._goal_yaw = observation['test_setting']['goal'][self.head_key][0]

    def process(self, collision_lookup, observation, traj, plan_all=False, scenario_to_test=None):
        scene_name = observation['test_setting']['scenario_name']
        goal_x = observation['test_setting']["goal"]["x"]
        goal_y = observation['test_setting']["goal"]["y"]
        start_x = observation['vehicle_info']["ego"]["x"]
        start_y = observation['vehicle_info']["ego"]["y"]
        local_x_range = observation['hdmaps_info']['image_mask'].bitmap_info['bitmap_mask_PNG']['UTM_info']['local_x_range']
        local_y_range = observation['hdmaps_info']['image_mask'].bitmap_info['bitmap_mask_PNG']['UTM_info']['local_y_range']
        
        min_dist = np.inf
        for i in traj:
            x_1 = traj[i]['0.0']['x']
            y_1 = traj[i]['0.0']['y']
            dist_1_start = math.sqrt((start_x - x_1) ** 2 + (start_y - y_1) ** 2) * 10
            if dist_1_start < min_dist:
                min_dist = dist_1_start
        for i in traj:
            observation['hdmaps_info']['image_mask'].image_ndarray = back_cars2_obstacles(traj[i]['0.0']['x'], traj[i]['0.0']['y'], local_x_range, local_y_range, observation['hdmaps_info']['image_mask'].image_ndarray, dist_1_start=min_dist-1, value=False)
            
        observation['hdmaps_info']['image_mask'].image_ndarray_local = self.get_local_image(observation)
        image_ndarray_new = observation['hdmaps_info']['image_mask'].image_ndarray

        utm_local_range = observation['hdmaps_info']['image_mask'].bitmap_local_info['utm_local_range']
        x_min, y_min, x_max, y_max = utm_local_range[0], utm_local_range[1], utm_local_range[2], utm_local_range[3]
        x_margin, y_margin = 20, 20
        x_min, x_max, y_min, y_max = x_min - x_margin, x_max + x_margin, y_min - y_margin, y_max + y_margin

        collision_ini_org = False
        collision_ini = check_car_collision(goal_x, goal_y, [observation['test_setting']['goal'][self.head_key][0]] * 4,
                                            collision_lookup, observation,
                                            image_ndarray_new)

        if collision_ini:
            goal_x += 10 * np.cos(observation['test_setting']['goal'][self.head_key][0] + np.pi)
            goal_y += 10 * np.sin(observation['test_setting']['goal'][self.head_key][0] + np.pi)
            collision_ini = check_car_collision(goal_x, goal_y, [observation['test_setting']['goal'][self.head_key][0]] * 4,
                                                collision_lookup, observation,
                                                image_ndarray_new)

            goal_x = observation['test_setting']["goal"]["x"]
            goal_y = observation['test_setting']["goal"]["y"]
            extra_dist = 1
            if not collision_ini:
                collision_ini = True
                while collision_ini:
                    goal_x += 0.01 * np.cos(observation['test_setting']['goal'][self.head_key][0] + np.pi)
                    goal_y += 0.01 * np.sin(observation['test_setting']['goal'][self.head_key][0] + np.pi)
                    collision_ini = check_car_collision([np.mean(goal_x)], [np.mean(goal_y)],
                                                        [observation['test_setting']['goal'][self.head_key][0]],
                                                        collision_lookup, observation,
                                                        image_ndarray_new)

                goal_x += extra_dist * np.cos(observation['test_setting']['goal'][self.head_key][0] + np.pi)
                goal_y += extra_dist * np.sin(observation['test_setting']['goal'][self.head_key][0] + np.pi)
            else:
                collision_ini = True
                print('终点距离障碍物很近')
                collision_ini_org = collision_ini
                while collision_ini:
                    goal_x += 0.01 * np.cos(observation['test_setting']['goal'][self.head_key][0])
                    goal_y += 0.01 * np.sin(observation['test_setting']['goal'][self.head_key][0])
                    collision_ini = check_car_collision([np.mean(goal_x)], [np.mean(goal_y)],
                                                        [observation['test_setting']['goal'][self.head_key][0]],
                                                        collision_lookup, observation,
                                                        image_ndarray_new)

                goal_x += extra_dist * np.cos(observation['test_setting']['goal'][self.head_key][0])
                goal_y += extra_dist * np.sin(observation['test_setting']['goal'][self.head_key][0])

            if 'new_goal' not in observation['test_setting']:
                observation['test_setting']['new_goal'] = {}
            observation['test_setting']['new_goal']['x'] = goal_x
            observation['test_setting']['new_goal']['y'] = goal_y
            observation['test_setting']['new_goal']['head'] = observation['test_setting']['goal'][self.head_key]

            # plt.figure(figsize=(6.2, 5), dpi=400)
            # plot_path_planned(observation, [], start_x, start_y, goal_x, goal_y,
            #                   [], x_min, x_max, y_min, y_max, x_1, y_1, [], scene_name)
            # plt.savefig(file_path)
        """规划器主函数.
        注：该函数功能设计举例
        1) 进行实时轨迹规划;
        2) 路径、速度解耦方案:给出局部轨迹规划器的实时求解结果--待行驶路径、待行驶速度；

        输入:observation——环境信息;
        输出: 路径、速度解耦方案:给出局部轨迹规划器的实时求解--
            待行驶路径（离散点序列）、待行驶速度（与路径点序列对应的速度离散点序列）
        """

        # grid_size = 400  # 网格大小
        # pixel_local_range = observation['hdmaps_info']['image_mask'].bitmap_local_info['pixel_local_range']
        # image_ndarray_local = image_ndarray_new[pixel_local_range[1]:pixel_local_range[3], pixel_local_range[0]:pixel_local_range[2]]
        # image_ndarray_local = downsample_image(image_ndarray_local, (grid_size, grid_size))
        # plot_bit_path(image_ndarray_local, [])
        # plt.show()

        # 设置目标速度
        target_speed = 7
        target_speed_backward = 10 / 3.6

        start = [observation['vehicle_info']['ego']['x'],
                 observation['vehicle_info']['ego']['y'],
                 observation['vehicle_info']['ego']['yaw_rad']]
        goal = [np.mean(goal_x),
                np.mean(goal_y),
                observation['test_setting']['goal'][self.head_key][0]]
        spd_planned = []
        # path_planned = self.get_ego_reference_path_to_goal(observation)
        # if observation["test_setting"]["scenario_type"] == "loading" and not plan_all:
        #     path_planned = self.get_main_road(observation)
        #     sorted_path = [path_planned[0]]  # 将起点加入已排序路径中
        #     while len(sorted_path) < len(path_planned):
        #         # 找出离已排序路径的最后一个点最近的点
        #         nearest_point = min((point for point in path_planned if point not in sorted_path),
        #                             key=lambda point: distance(sorted_path[-1], (point[0], point[1])))
        #         sorted_path.append(nearest_point)
        #     path_planned = sorted_path
        #     print('len(main_road)', len(path_planned))
        #     path_planned, keypoint_ind, astar_path = self.connect_path_to_goal(path_planned, goal, collision_lookup, observation, image_ndarray_new)
        #     # 注意：connect_path_to_goal函数已经处理了astar_path，不需要再次处理
            
        #     # 为路径上的每个点设置速度
        #     for k in range(len(path_planned)):
        #         if k < 50:
        #             spd_planned.append(target_speed / 50 * k)
        #         elif k >= 50 and k <= keypoint_ind - 100:
        #             spd_planned.append(target_speed)
        #         elif (k > keypoint_ind - 100) and (k <= keypoint_ind):
        #             spd_planned.append(target_speed / 100 * (keypoint_ind - k))
        #         elif k > keypoint_ind and k <= keypoint_ind + 100:
        #             spd_planned.append(-target_speed_backward / 120 * (k - keypoint_ind + 20))
        #         elif k > keypoint_ind + 100 and k < len(path_planned) - 100:
        #             spd_planned.append(-target_speed_backward)
        #         elif k >= len(path_planned) - 100:
        #             spd_planned.append(-target_speed_backward / 100 * (len(path_planned) - k - 1))

        if observation["test_setting"]["scenario_type"] in ["mission", "loading"]:
            path_planned_bak = None
            astar_star = [observation['vehicle_info']['ego']['x'],
                          observation['vehicle_info']['ego']['y'],
                          observation['vehicle_info']['ego']['yaw_rad']]
            
            astar_path = ([], [], [])
            # for max_steer in [30, 50]:    # 注释这里用来调试rrt
            #     print(f'尝试使用max_steer={max_steer}')
            #     start_time = time.time()
            #     astar_path = hybrid_a_star_planning(astar_star, goal, collision_lookup, observation, 2.0, 15, True,
            #                                         image_ndarray_new, iter_number=1000, MOTION_RESOLUTION=0.1, max_steer=max_steer)
            #     print(f'用时{time.time() - start_time:.3f}秒')
                
            #     if hasattr(astar_path, "xlist"):    # 如果astar_path有xlist属性，则跳出循环
            #         break

            # if not hasattr(astar_path, "xlist"):    # 调试时注释这里，直接使用已保存的路径
            print(f"###log### 先使用Fast-RRT*规划路径")
            start_time = time.time()
            best_path = None
            best_spd = None
            min_cost = float('inf')
            seed = 0
            num_expand = None
            num_expand_list = []
            downsampled_image_list, goal_area_polygon_org, goal_mask, goal_area_polygon = None, None, None, None
            for i in range(5):
                seed_try = seed + i * 999 if seed is not None else i
                num_expand = max(num_expand_list) + 2 if num_expand_list else num_expand
                path_try, spd_try, max_steer_rrt, num_expand_used, downsampled_image_list, goal_area_polygon_org, goal_mask, goal_area_polygon = Planner_backup(observation, seed=seed_try, num_expand=num_expand, 
                        downsampled_image_list=downsampled_image_list, goal_area_polygon_org=goal_area_polygon_org, goal_mask=goal_mask, goal_area_polygon=goal_area_polygon, collision_ini=collision_ini_org)
                num_expand_list.append(num_expand_used)
                if path_try is not None:
                    cost = max_steer_rrt * 10 + len(path_try) - num_expand_used * 150
                    if cost < min_cost:
                        best_path = path_try
                        best_spd = spd_try
                        min_cost = cost
                if max_steer_rrt < 7:
                    break
            path_planned_bak, spd_planned_bak = best_path, best_spd
            print(f'Fast-RRT*用时{time.time() - start_time:.3f}秒')

            if not path_planned_bak:
                return None, None

            # os.makedirs(os.path.dirname(file_path), exist_ok=True)  # 保存路径用于调试，不调试时记得注释
            # np.save(file_path, path_planned_bak)
            # print(f'path_planned_bak 已保存至 {file_path}')
            
            # file_path = os.path.join(dir_algorithm, "outputs", "path_npy", 'rrt', f'{scene_name}.npy')  # 保存路径用于调试，不调试时记得注释
            # if os.path.exists(file_path):
            #     path_planned_bak = np.load(file_path).tolist()
            #     print(f'path_planned_bak 已加载至 {file_path}')

            # path_planned, keypoint_ind, astar_path = self.connect_path_to_goal(path_planned_bak, goal, collision_lookup, observation, image_ndarray_new, no_back=True)

            # if not hasattr(astar_path, "xlist"):
            #     path_planned, keypoint_ind, astar_path = self.connect_path_to_goal(path_planned_bak, goal, collision_lookup, observation, image_ndarray_new, no_back=True, max_steer=50)
            
            if not hasattr(astar_path, "xlist"):
                path_planned, keypoint_ind, astar_path = self.connect_path_to_goal(path_planned_bak, goal, collision_lookup, observation, image_ndarray_new, max_steer=15, iter_number=80)

            if not hasattr(astar_path, "xlist"):
                path_planned, keypoint_ind, astar_path = self.connect_path_to_goal(path_planned_bak, goal, collision_lookup, observation, image_ndarray_new, iter_number=100)
            
            if not hasattr(astar_path, "xlist"):
                path_planned, keypoint_ind, astar_path = self.connect_path_to_goal(path_planned_bak, goal, collision_lookup, observation, image_ndarray_new, max_steer=50)

            path_planned_srt, astar_path = self.connect_path_to_start(path_planned, start, collision_lookup, observation, image_ndarray_new) if self.check_yaw_difference(path_planned, start) > 10 and self.check_yaw_difference(path_planned, start) <= 90 else path_planned, ([], [], [])
            # if self.check_yaw_difference(path_planned, start) > 90:
            #     print("###log### 开始以退为进")
            #     start_b = [start[0], start[1], (observation['vehicle_info']['ego']['yaw_rad'] + np.pi) % (2 * np.pi)]
            #     path_planned_srt, astar_path = self.connect_path_to_start(path_planned, start_b, collision_lookup, observation, image_ndarray_new)
            #     # path_planned_srt = [p[:3] + [not p[3]] + p[4:] for p in path_planned_srt]   # 备用写法
            #     for p in path_planned_srt:
            #         p[3] = not p[3]
            path_planned = path_planned_srt if hasattr(astar_path, "xlist") else path_planned
            # path_planned = path_planned_bak
            # path_planned = self.connect_path_to_start(path_planned, start, collision_lookup, observation, image_ndarray_new) if self.check_yaw_difference(path_planned, start) > 10 else path_planned
            # keypoint_ind = len(path_planned) - 1

            # elif hasattr(astar_path, "xlist") and not path_planned_bak: # 如果astar_path有,而path_planned_bak为空
            #     path_planned = []
            #     keypoint_ind = 0  # 初始化为0而不是len(path_planned)，因为此时path_planned为空
            #     for j in range(1, len(astar_path.xlist)):
            #         path_planned.append(
            #             [astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j], astar_path.directionlist[j]])
                    
            #         if astar_path.directionlist[j] == True and astar_path.directionlist[j + 1] == False:
            #             print("keypoint:", [astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j], astar_path.directionlist[j]])
            #             keypoint_ind = j - 1  # 直接赋值而不是累加，因为这是唯一的关键点

            N = len(path_planned)   
            print(f"keypoint_ind: {keypoint_ind}")     
            if N >= keypoint_ind + 202 and keypoint_ind >= 150:
                for k in range(len(path_planned)):
                    if k < 50:
                        spd_planned.append(target_speed / 50 * k)
                    elif k >= 50 and k <= keypoint_ind - 100:
                        spd_planned.append(target_speed)
                    elif (k > keypoint_ind - 100) and (k <= keypoint_ind):
                        spd_planned.append(target_speed / 100 * (keypoint_ind - k))
                    elif k > keypoint_ind and k <= keypoint_ind + 100:
                        spd_planned.append(-target_speed_backward / 120 * (k - keypoint_ind + 20))
                    elif k > keypoint_ind + 100 and k < len(path_planned) - 100:
                        spd_planned.append(-target_speed_backward)
                    elif k >= len(path_planned) - 100:
                        spd_planned.append(-target_speed_backward / 100 * (len(path_planned) - k - 1))
            else:
                for k in range(len(path_planned)):
                    if k < (keypoint_ind/2):
                        # 前半程匀速前进
                        spd_planned.append(target_speed)
                    elif k <= keypoint_ind:
                        # 后半程线性从 target_speed 减到 0
                        spd_planned.append(target_speed * (2 - 2 * k / keypoint_ind))

                    # —— 倒车部分 ——  
                    # 倒车分两段：先从 0 加速到 -target_speed_backward，再从 -target_speed_backward 减速回 0
                    elif k <= keypoint_ind + (len(path_planned) - keypoint_ind)/2:
                        # 倒车加速段： k 从 keypoint_ind→keypoint+half，总长度 = len-path_pt - keypt
                        spd_planned.append(
                            -target_speed_backward
                            * (2 * (k - keypoint_ind) / (len(path_planned) - keypoint_ind))
                        )
                    else:
                        # 倒车减速段：从 -target_speed_backward 线性回到 0
                        spd_planned.append(
                            -target_speed_backward
                            * (2 - 2 * (k - keypoint_ind) / (len(path_planned) - keypoint_ind))
                        )              


            for i in range(1, len(path_planned)):
                path_planned[i].append(
                    calculate_curvature(path_planned[i - 1][0], path_planned[i - 1][1], path_planned[i - 1][2],
                                                path_planned[i][0], path_planned[i][1], path_planned[i][2]))
                
        elif observation["test_setting"]["scenario_type"] == "unloading":
            goal = [np.mean(observation['test_setting']['goal']['x']),
                    np.mean(observation['test_setting']['goal']['y']),
                    observation['test_setting']['goal'][self.head_key][0]]
            path_planned = self.get_main_road(observation, False)
            astar_star = [observation['vehicle_info']['ego']['x'],
                          observation['vehicle_info']['ego']['y'],
                          observation['vehicle_info']['ego']['yaw_rad']]
            nearest_point, point_ind, min_distance, flag = self.find_nearest_point(path_planned, astar_star)
            if flag:
                astar_path = ([], [], [])
                for max_steer in [30, 50]:  # 依次尝试不同转向角度
                    print(f'尝试使用max_steer={max_steer}')
                    start_time = time.time()
                    astar_path = hybrid_a_star_planning(astar_star, goal, collision_lookup, observation, 2.0, 15, True,
                                                        image_ndarray_new, iter_number=1000, MOTION_RESOLUTION=0.1, max_steer=max_steer)
                    print(f'用时{time.time() - start_time:.3f}秒')
                    if hasattr(astar_path, "xlist"):    # 如果astar_path有xlist属性，则跳出循环
                        break

                path_planned = []
                for j in range(1, len(astar_path.xlist)):
                    path_planned.append(
                        [astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j], astar_path.directionlist[j]])
                cute_need_path = path_planned
                cute_need_path1 = cute_need_path[::3]
                for _ in range(50):
                    path_planned = self.cute_path(cute_need_path1,collision_lookup,image_ndarray_new,observation)
                for k in range(len(path_planned)):
                    if k < 50:
                        spd_planned.append(target_speed / 50 * k)
                    else:
                        spd_planned.append(target_speed)
            else:
                dist = 0
                while point_ind < len(path_planned):

                    dist += distance(path_planned[point_ind], path_planned[point_ind - 1])
                    point_ind += 1
                    # print(path_planned[point_ind+1][0])
                    if dist > 20:
                        break
                print("min_distance:", min_distance)
                print("nearest_point", nearest_point)
                path_planned = path_planned[point_ind - 1:]

                if len(path_planned) < 200:
                    handover = path_planned[-1]

                    astar_path = ([], [], [])
                    for max_steer in [30, 50]:  # 依次尝试不同转向角度
                        print(f'尝试使用max_steer={max_steer}')
                        start_time = time.time()
                        astar_path = hybrid_a_star_planning(astar_star, handover, collision_lookup, observation, 2.0, 15, False,
                                                            image_ndarray_new, iter_number=1000, MOTION_RESOLUTION=0.1, max_steer=max_steer)
                        print(f'用时{time.time() - start_time:.3f}秒')
                        if hasattr(astar_path, "xlist"):    # 如果astar_path有xlist属性，则跳出循环
                            break

                    path_planned = []
                    path_planned1 = []
                    for j in range(1, len(astar_path.xlist)):
                        path_planned1.append([astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j],
                                              astar_path.directionlist[j]])
                    cute_need_path = path_planned1 + path_planned
                    cute_need_path1 = cute_need_path[::10]
                    for _ in range(100):
                            path_planned = self.cute_path(cute_need_path1,collision_lookup,image_ndarray_new,observation)
                else:
                    handover_index = 0
                    # while handover_index <= len(path_planned):
                    #     handover = path_planned[handover_index]
                    #     try:
                    #         astar_path = hybrid_a_star_planning(astar_star, handover, collision_lookup, observation, 2.0, 15,False,image_ndarray_new,iter_number=100000,MOTION_RESOLUTION=0.1)
                    #         if hasattr(astar_path, "xlist"):
                    #             break
                    #     except:
                    #         pass
                    #     handover_index += 50
                    # path_planned = path_planned[len(path_planned)-handover_index:]
                    handover = path_planned[handover_index]

                    astar_path = ([], [], [])
                    for max_steer in [30, 50]:  # 依次尝试不同转向角度
                        print(f'尝试使用max_steer={max_steer}')
                        start_time = time.time()
                        astar_path = hybrid_a_star_planning(astar_star, handover, collision_lookup, observation, 2.0, 15, False,
                                                            image_ndarray_new, iter_number=1000, MOTION_RESOLUTION=0.1, max_steer=max_steer)
                        print(f'用时{time.time() - start_time:.3f}秒')
                        if hasattr(astar_path, "xlist"):    # 如果astar_path有xlist属性，则跳出循环
                            break

                    for i in range(len(path_planned)):
                        path_planned[i] = path_planned[i][0:3]
                        path_planned[i].append(True)
                    path_planned1 = []
                    for j in range(1, len(astar_path.xlist)):
                        path_planned1.append([astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j],
                                              astar_path.directionlist[j]])
                    if len(path_planned) <= 200:
                        cute_need_path = path_planned1 + path_planned
                        cute_need_path1 = cute_need_path[::10]
                        for _ in range(200):
                            path_planned = self.cute_path(cute_need_path1,collision_lookup,image_ndarray_new,observation)
                    else:
                        cute_need = path_planned[0:149]
                        cute_need_path = path_planned1 + cute_need
                        path_planned_new = path_planned[150:len(path_planned)]
                        cute_need_path1 = cute_need_path[::20]
                        for _ in range(200):
                            cute_path = self.cute_path(cute_need_path1,collision_lookup,image_ndarray_new,observation)
                        path_planned = cute_path + path_planned_new
                # if len(path_planned) == 1138:
                #     path_planned = []
                #     i = "0.0"
                #     # print(i)
                #     backvehicle_traj = traj[1]
                #     while True:
                #         xback = backvehicle_traj[i]['x']
                #         yback = backvehicle_traj[i]['y']
                #         image_ndarray_new = back_cars2_obstacles_1(xback, yback, local_x_range, local_y_range,
                #                                                    image_ndarray_new,
                #                                                     value=False)
                #         result = float(i) + 0.1
                #         i = str(round(result, 1))
                #         if ((result * 10) > (len(backvehicle_traj) - 302)):
                #             image_ndarray_new = start2_obstacles_1(start_x, start_y, local_x_range, local_y_range,
                #                                                    image_ndarray_new,
                #                                                     value=True)
                #             break
                #     astar_path = hybrid_a_star_planning(astar_star, goal, collision_lookup, observation, 2.0, 15, True,
                #                                         image_ndarray_new, iter_number=10000, MOTION_RESOLUTION=1,max_steer=90
                #                                         )
                #     if astar_path == ([], [], []):
                #         astar_path = hybrid_a_star_planning(astar_star, goal, collision_lookup, observation, 2.0, 15, True,
                #                                         image_ndarray_new, iter_number=10000, MOTION_RESOLUTION=1,max_steer=90
                #                                         )
                #     # if astar_path == ([], [], []):
                #     #     x_backv = observation['vehicle_info'][1]['x']
                #     #     y_backv = observation['vehicle_info'][1]['y']
                #     #     goal_m = (x_backv,y_backv)
                #     #     astar_path1 = hybrid_a_star_planning(astar_star, goal_m, collision_lookup, observation, 2.0,
                #     #                                         15, True, image_ndarray_new, 10000, 1,
                #     #                                         max_steer=90)
                #     #     astar_path2 = hybrid_a_star_planning(goal_m, goal, collision_lookup, observation, 2.0,
                #     #                                         15, True, image_ndarray_new, 10000, 1,
                #     #                                         max_steer=90)
                #     #     for i in range(1, len(astar_path1.xlist)):
                #     #         path_planned1.append([astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j],
                #     #                             astar_path.directionlist[j]])
                #     #     for j in range(1, len(astar_path2.xlist)):
                #     #         path_planned2.append([astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j],
                #     #                             astar_path.directionlist[j]])
                #     #     path_planned = path_planned1 + path_planned2
                #     # else:
                #     #     for j in range(1, len(astar_path.xlist)):
                #     #         path_planned.append([astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j],
                #     #                             astar_path.directionlist[j]])
                #     for j in range(1, len(astar_path.xlist)):
                #             path_planned.append([astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j],
                #                                 astar_path.directionlist[j]])
                #     cute_need_path1 = path_planned[::3]
                #     for _ in range(100):
                #         path_planned = self.cute_path(cute_need_path1,collision_lookup,image_ndarray_new,observation)
                if 'vehicle_info' in observation and 1 in observation['vehicle_info'] and dist_1_start < 300:
                    for k in range(len(path_planned)):
                        if k < 100:
                            spd_planned.append(target_speed / 100 * k / 3)
                        elif 100 <= k < 150:
                            spd_planned.append(target_speed / 50 * (k - 100) * 2 / 3 + target_speed / 3)
                        else:
                            spd_planned.append(target_speed)
                else:
                    for k in range(len(path_planned)):
                        if k < 50:
                            spd_planned.append(target_speed / 50 * k)
                        else:
                            spd_planned.append(target_speed)

        for i in range(1, len(path_planned)):
            path_planned[i] = list(path_planned[i])
            path_planned[i].append(
                calculate_curvature(path_planned[i - 1][0], path_planned[i - 1][1], path_planned[i - 1][2],
                                         path_planned[i][0], path_planned[i][1], path_planned[i][2]))

        if do_plot == True:
            file_path = os.path.join(dir_algorithm, "outputs", "plot", scene_name + ".png")
            os.makedirs(os.path.dirname(file_path), exist_ok=True)

            plt.figure(figsize=(6.2, 5), dpi=400)
            plot_path_planned(observation, path_planned, start_x, start_y, goal_x, goal_y, astar_path, x_min, x_max, y_min, y_max, traj, spd_planned, scene_name)
            plt.savefig(file_path)
            print(f"路径规划结果已保存至{file_path}")
            plt.close()

        return path_planned[1:], spd_planned[1:]

    def get_local_image(self, observation):
        """
        从完整图像中获取局部图像。
        
        参数:
        - observation: 包含图像信息的观测数据
        
        返回:
        - image_ndarray_local: 裁剪后的局部图像
        """
        # 获取原始图像
        image_ndarray = observation['hdmaps_info']['image_mask'].image_ndarray
        
        # 获取局部范围信息
        pixel_local_range = observation['hdmaps_info']['image_mask'].bitmap_local_info['pixel_local_range']
        
        # 使用pixel_local_range裁剪图像
        # pixel_local_range格式为 [x_min, y_min, x_max, y_max]
        image_ndarray_local = image_ndarray[pixel_local_range[1]:pixel_local_range[3], pixel_local_range[0]:pixel_local_range[2]]
    
        return image_ndarray_local
    
    def cute_path(self, bit_path,collision_lookup,image_ndarray_new,observation):
        i_smo = 1
        local_x_range = observation['hdmaps_info']['image_mask'].bitmap_info['bitmap_mask_PNG']['UTM_info']['local_x_range']
        local_y_range = observation['hdmaps_info']['image_mask'].bitmap_info['bitmap_mask_PNG']['UTM_info']['local_y_range']
        while i_smo < len(bit_path) - 1:

            point1 = bit_path[i_smo - 1]
            point2 = bit_path[i_smo + 1]

            num_points = 1
            x_interp = (point1 [0] + point2[0])/2
            y_interp = (point1[1] + point2[1])/2
            yaw_interp = (point1[2] + point2[2])/2
            dir = 1
            # observation['vehicle_info']["ego"]["shape"]["length"] = 10000
            # observation['vehicle_info']["ego"]["shape"]["width"] = 10000
            collision = collision_lookup.collision_detection(x_interp-local_x_range[0],
                                                    y_interp-local_y_range[0],
                                                    yaw_interp,
                                                    image_ndarray_new)
            # collision_mat = np.zeros(x_interp.shape, dtype=bool)
            # for i_check in range(len(x_interp)):
            #     x_check = int(round(x_interp[i_check]))
            #     y_check = int(round(y_interp[i_check]))
            #     collision_mat[i_check] = downsampled_image_2[y_check, x_check]

            # collision = np.any(collision_mat == True)  

            if not collision:
                bit_path_smo = bit_path[i_smo]
                bit_path[i_smo] = (x_interp, y_interp,yaw_interp,dir)
            i_smo += 1

        return bit_path
    
    def get_main_road(self, observation, bo_in=True):
        """获取HD Map中ego车到达铲装平台的参考路径.
        输入:observation——环境信息;
        输出:ego车到达目标区域的参考路径(拼接后).
        """
        main_road = {"polygon-14": [["path-2", "path-3", "path-4", "path-48", "path-50", "path-43", "path-44"],
                                    ["path-13", "path-14", "path-58", "path-31", "path-37", "path-38", "path-39"]],
                     "polygon-27": [
                         ["path-36", "path-5", "path-6", "path-7", "path-8", "path-9", "path-70", "path-71", "path-72",
                          "path-73"],
                         ["path-17", "path-18", "path-19", "path-20", "path-21", "path-65", "path-66", "path-67",
                          "path-64", "path-80", "path-81"]],
                     "polygon-29": [
                         ["path-36", "path-5", "path-6", "path-7", "path-8", "path-9", "path-84", "path-85", "path-74",
                          "path-75"],
                         ["path-22", "path-23", "path-59", "path-65", "path-66", "path-67", "path-64", "path-80",
                          "path-81"]],
                     "polygon-25": [["path-36", "path-5", "path-6", "path-7", "path-8", "path-9", "path-68", "path-69"],
                                    ["path-15", "path-16", "path-65", "path-66", "path-67", "path-64", "path-80",
                                     "path-81"]],
                     "polygon-10": [["path-2", "path-45", "path-46", "path-47", "path-40", "path-41"],
                                    ["path-77", "path-10", "path-11", "path-27", "path-28", "path-25", "path-26",
                                     "path-39"]]}
        #################更新参数#################
        self._ego_x = observation['vehicle_info']['ego']['x']
        self._ego_y = observation['vehicle_info']['ego']['y']
        self._ego_v = observation['vehicle_info']['ego']['v_mps']
        self._ego_yaw = observation['vehicle_info']['ego']['yaw_rad']
        #################定位主车和目标点所在几何#################
        if bo_in:
            ego_polygon_token = observation['hdmaps_info']['tgsc_map'].get_polygon_token_using_node(self._ego_x,
                                                                                                    self._ego_y)
            ego_polygon_id = int(ego_polygon_token.split('-')[1])
            print("ego_polygon_token:", ego_polygon_token)
            goal_polygon_token = observation['hdmaps_info']['tgsc_map'].get_polygon_token_using_node(self._goal_x,
                                                                                                     self._goal_y)
            print("goal_polygon_token:", goal_polygon_token)
            ego_dubinspose_token = self.get_dubinspose_token_from_polygon \
                (observation, (self._ego_x, self._ego_y, self._ego_yaw), ego_polygon_token)
            print("ego_dubinspose_token:", ego_dubinspose_token)
        else:
            ego_polygon_token = observation['hdmaps_info']['tgsc_map'].get_polygon_token_using_node(self._goal_x,
                                                                                                    self._goal_y)
            ego_polygon_id = int(ego_polygon_token.split('-')[1])
            print("ego_polygon_token:", ego_polygon_token)
            goal_polygon_token = observation['hdmaps_info']['tgsc_map'].get_polygon_token_using_node(self._ego_x,
                                                                                                     self._ego_y)
            print("goal_polygon_token:", goal_polygon_token)
            ego_dubinspose_token = self.get_dubinspose_token_from_polygon \
                (observation, (self._goal_x, self._goal_y, self._goal_yaw), ego_polygon_token)
            print("ego_dubinspose_token:", ego_dubinspose_token)
        #################获取目标车最匹配的dubinspose#################

        # ego_dubinspose_id = int( ego_dubinspose_token.split('-')[1])
        # ego_dubinspose_token 作为 起点、终点的path拿到
        link_referencepath_tokens_ego_polygon = observation['hdmaps_info']['tgsc_map'].polygon[ego_polygon_id][
            'link_referencepath_tokens']
        # 去除掉 不包含 ego_dubinspose_token 的 path
        for _, path_token in enumerate(link_referencepath_tokens_ego_polygon):
            path_id = int(path_token.split('-')[1])
            link_dubinspose_tokens = observation['hdmaps_info']['tgsc_map'].reference_path[path_id][
                'link_dubinspose_tokens']
            if ego_dubinspose_token not in link_dubinspose_tokens:
                pass
            else:
                only_one_path_token = path_token
                only_one_path_id = path_id
        print("path_token:", path_token)
        path_connected = []
        for road in main_road[goal_polygon_token]:
            if only_one_path_token in road:
                ind = road.index(only_one_path_token)
                terminal_path_id = int(road[ind].split('-')[1])
                terminal_path = observation['hdmaps_info']['tgsc_map'].reference_path[terminal_path_id]['waypoints']
                if bo_in:
                    _, nearest_point_ind, _, _ = self.find_nearest_point(terminal_path, [self._ego_x, self._ego_y])
                    terminal_path = terminal_path[nearest_point_ind:]
                    road_1 = road[ind + 1:len(road)]
                    for road_segment in road_1:
                        path_id = int(road_segment.split('-')[1])
                        path_connected += observation['hdmaps_info']['tgsc_map'].reference_path[path_id]['waypoints']
                    path_connected = terminal_path + path_connected
                else:
                    _, nearest_point_ind, _, _ = self.find_nearest_point(terminal_path, [self._goal_x, self._goal_y])
                    terminal_path = terminal_path[0:nearest_point_ind + 1]
                    road_1 = road[:ind]
                    for road_segment in road_1:
                        path_id = int(road_segment.split('-')[1])
                        path_connected += observation['hdmaps_info']['tgsc_map'].reference_path[path_id]['waypoints']
                    path_connected = path_connected + terminal_path
        return path_connected

    def get_dubinspose_token_from_polygon(self, observation, veh_pose: Tuple[float, float, float], polygon_token: str):
        id_polygon = int(polygon_token.split('-')[1])
        link_dubinspose_tokens = observation['hdmaps_info']['tgsc_map'].polygon[id_polygon]['link_dubinspose_tokens']
        dubinsposes_indicators = []
        for token in link_dubinspose_tokens:
            id_dubinspose = int(token.split('-')[1])
            dx = observation['hdmaps_info']['tgsc_map'].dubins_pose[id_dubinspose]['x'] - veh_pose[0]
            dy = observation['hdmaps_info']['tgsc_map'].dubins_pose[id_dubinspose]['y'] - veh_pose[1]
            dyaw = observation['hdmaps_info']['tgsc_map'].dubins_pose[id_dubinspose]['yaw'] - veh_pose[2]
            distance = math.sqrt(dx ** 2 + dy ** 2)
            if distance < 3 and abs(dyaw) < 1e-2:
                return token

    def find_nearest_point(self, path, point):
        """
        在路径上找到给定点的最近点。

        参数:
        - path: 路径点的列表，每个点是一个(x, y)元组。
        - point: 给定点，一个(x, y)元组。

        返回:
        - nearest_point: 路径上最近的点。
        - min_distance: 到给定点的最小距离。
        """

        # 初始化最小距离和最近点
        min_distance = float('inf')
        nearest_point = None
        point_ind = 0
        # print(path)
        # 遍历路径上的每个点
        for i in range(len(path)):
            path_point = path[i]
            # 计算当前点与给定点之间的欧几里得距离
            distance = math.sqrt((path_point[0] - point[0]) ** 2 + (path_point[1] - point[1]) ** 2)

            # 更新最小距离和最近点
            if distance < min_distance:
                point_ind = i
                nearest_point = path_point
                min_distance = distance
        if nearest_point == None:
            flag = True
        else:
            flag = False
        return nearest_point, point_ind, min_distance, flag

    def calculate_path_length(self, path):
        """
        计算路径的总长度。
        参数:
        - path: 路径点的列表，每个点是一个(x, y)元组或列表。
        返回:
        - 路径的总长度（float）
        """
        if hasattr(path, "xlist"):  # 如果是Astar，则转为list
            path_planned = []
            for j in range(1, len(path.xlist)):
                path_planned.append([path.xlist[j], path.ylist[j]])
            path = path_planned

        if not path or len(path) < 2:
            return 0.0
        total_length = 0.0
        for i in range(1, len(path)):
            total_length += distance(path[i-1], path[i])
        return total_length
    def connect_path_to_goal(self, path_planned, goal, collision_lookup, observation, image_ndarray_new, retreat_dist=20, no_back=False, max_steer=30, iter_number=1000):
        """
        将规划路径与目标点连接，处理路径的裁剪和A*路径规划
        
        参数:
        - path_planned: 已规划的路径
        - goal: 目标点 [x, y, yaw]
        - collision_lookup: 碰撞检测器
        - observation: 环境观测
        - image_ndarray_new: 障碍物栅格地图
        - retreat_dist: 从最近点回退的距离，默认30米
        
        返回:
        - path_planned: 连接后的路径
        - keypoint_ind: 关键点索引
        """
        print("###log### connect_path_to_goal")
        # 找到路径上距离目标点最近的点
        _, point_ind, min_distance, _ = self.find_nearest_point(path_planned, goal)
        print("min_distance:", min_distance)
        
        # 计算路径总长度并动态调整retreat_dist
        path_length = self.calculate_path_length(path_planned)
        print(f"RRT路径总长度{path_length}")
        print(f"path_planned的总长度: {path_length}")
        if path_length < 2 * retreat_dist:
            retreat_dist = 15
        if path_length < 2 * retreat_dist:
            retreat_dist = 10
        if path_length < 2 * retreat_dist:
            retreat_dist = 5
        print(f"动态调整后的retreat_dist: {retreat_dist}")
        
        # 从最近点开始向后回退一定距离
        dist = 0
        while point_ind:
            dist += distance(path_planned[point_ind], path_planned[point_ind - 1])
            point_ind -= 1
            if dist > retreat_dist:
                break
        
        print(f"裁切之前的len(path_planned): {len(path_planned)}")
        # 裁剪路径，只保留到回退点的部分
        path_planned = path_planned[0:point_ind + 1]
        print(f"根据最近点再退{retreat_dist}米裁切后的len(path_planned): {len(path_planned)}")
        
        # 将路径点格式统一，添加方向标志
        for i in range(len(path_planned)):
            path_planned[i] = path_planned[i][0:3]  # 将路径中的每个点的维度截断为3
            path_planned[i].append(True)  # 添加一个布尔值True，表示路径上的点为前进方向
        
        # 从路径末尾寻找一个合适的交接点进行A*规划

        goal_b = [goal[0], goal[1], (observation['test_setting']['goal'][self.head_key][0] + np.pi) % (2 * np.pi)]
        goal_list = [goal]
        for goal_r in goal_list:
            handover_index = -1
            is_Astar_short = True
            while handover_index >= -len(path_planned):
                handover = path_planned[handover_index]
                print(f'尝试使用handover_index={handover_index}, max_steer={max_steer}')

                astar_path = ([], [], [])
                # # 尝试不同的最大转向角
                # for max_steer in [30, 50]:
                # print(f'尝试使用max_steer={max_steer}')
                start_time = time.time()
                # if no_back and is_Astar_short:  # 不够短直接放弃
                if no_back:
                    astar_path = hybrid_a_star_noback(handover, goal_r, collision_lookup, observation, 2.0, 15, True,
                                                    image_ndarray_new, iter_number=iter_number, MOTION_RESOLUTION=0.1, max_steer=max_steer)
                # elif not no_back and is_Astar_short:  # 不够短直接放弃
                else:
                    astar_path = hybrid_a_star_planning(handover, goal_r, collision_lookup, observation, 2.0, 15, True,
                                                    image_ndarray_new, iter_number=iter_number, MOTION_RESOLUTION=0.1, max_steer=max_steer)
                print(f'用时{time.time() - start_time:.3f}秒')
                if hasattr(astar_path, "xlist"):
                    path_candidate = path_planned[0:len(path_planned) + handover_index] # 候选路径
                    is_Astar_short = self.calculate_path_length(astar_path) < 3 * (path_length - self.calculate_path_length(path_candidate))
                    print(f"A*路径是否够短→is_Astar_short: {is_Astar_short}")
                    if is_Astar_short:
                        break   # 退出while
                
                # 如果规划失败，尝试更前面的交接点
                handover_index -= retreat_dist * 10
                if handover_index <= -len(path_planned):
                    break
                if handover_index <= -retreat_dist * 30 - 1:
                    break

            if hasattr(astar_path, "xlist") and is_Astar_short:
                break   # 退出for

        if not hasattr(astar_path, "xlist"):
            print("A*寻路失败，返回空路径")
            return path_planned, len(path_planned), astar_path
        # 裁剪路径，只保留到交接点的部分
        path_planned = path_candidate
        print("A*寻路成功后裁切的len(path_planned):", len(path_planned))
        
        # 记录关键点索引，即A*路径开始的位置
        keypoint_ind = len(path_planned)
        is_back = False

        # 将A*路径添加到已有路径后面
        for j in range(1, len(astar_path.xlist)):
            path_planned.append(
                [astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j], astar_path.directionlist[j]])
            
            # 检测行驶方向变化点（前进变为后退）作为关键点
            if j < len(astar_path.xlist)-1 and astar_path.directionlist[j] == True and astar_path.directionlist[j + 1] == False:
                print("keypoint:", [astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j], astar_path.directionlist[j]])
                is_back = True
                keypoint_ind += j - 1  # 更新关键点索引
                
        keypoint_ind = keypoint_ind if is_back else len(path_planned)   # 如果没有倒车，取最后一个点
        print("keypoint_ind:", keypoint_ind)
        print("拼接A*后的len(path_planned):", len(path_planned))
        
        # 标记关键点
        if len(path_planned) > keypoint_ind:
            path_planned[keypoint_ind][3] = 2   # 用2标记关键点（前后行驶方向改变点）
        
        return path_planned, keypoint_ind, astar_path

    def connect_path_to_start(self, path_planned, start, collision_lookup, observation, image_ndarray_new, retreat_dist=20):
        """
        将起点与规划路径连接，处理路径的裁剪和A*路径规划
        
        参数:
        - path_planned: 已规划的路径
        - start: 起点 [x, y, yaw]
        - collision_lookup: 碰撞检测器
        - observation: 环境观测
        - image_ndarray_new: 障碍物栅格地图
        - retreat_dist: 从最近点回退的距离
        
        返回:
        - path_planned: 连接后的路径
        """
        print("###log### connect_path_to_start")
        # 找到路径上距离起点最近的点
        _, point_ind, min_distance, _ = self.find_nearest_point(path_planned, start)
        print("min_distance:", min_distance)
        
        # 计算路径总长度并动态调整retreat_dist
        path_length = self.calculate_path_length(path_planned)
        print(f"RRT路径总长度{path_length}")
        print(f"path_planned的总长度: {path_length}")
        if path_length < 2 * retreat_dist:
            retreat_dist = 15
        if path_length < 2 * retreat_dist:
            retreat_dist = 10
        if path_length < 2 * retreat_dist:
            retreat_dist = 5
        print(f"动态调整后的retreat_dist: {retreat_dist}")
        
        # 从最近点开始向前推进一定距离
        dist = 0
        is_Astar_short = True
        while point_ind < len(path_planned) - 1:
            dist += distance(path_planned[point_ind], path_planned[point_ind + 1])
            point_ind += 1
            if dist > retreat_dist:
                break
        
        # 裁剪路径，只保留回退点之后的部分
        path_planned = path_planned[point_ind:]
        print(f"根据最近点向前行进{retreat_dist}米裁切后的len(path_planned): {len(path_planned)}")
        
        # 将路径点格式统一，添加方向标志
        for i in range(len(path_planned)):
            path_planned[i] = path_planned[i][0:3]  # 将路径中的每个点的维度截断为3
            path_planned[i].append(True)  # 添加一个布尔值True，表示路径上的点为前进方向
        
        # 从路径起点寻找一个合适的交接点进行A*规划
        handover_index = 0
        while handover_index < len(path_planned):
            handover = path_planned[handover_index]
            print(f'尝试使用handover_index={handover_index}')
            
            astar_path = ([], [], [])
            # 尝试不同的最大转向角
            for max_steer in [30, 50]:
                start_time = time.time()
                if is_Astar_short:
                    astar_path = hybrid_a_star_noback(start, handover, collision_lookup, observation, 2.0, 15, True,
                                                    image_ndarray_new, iter_number=1000, MOTION_RESOLUTION=0.1, max_steer=max_steer)
                print(f'用时{time.time() - start_time:.3f}秒')
                if hasattr(astar_path, "xlist"):
                    path_candidate = path_planned[0:len(path_planned) + handover_index] # 候选路径
                    is_Astar_short = self.calculate_path_length(astar_path) < 2 * (path_length - self.calculate_path_length(path_candidate))
                    print(f"A*路径是否够短→is_Astar_short: {is_Astar_short}")
                    if is_Astar_short:
                        break
            
            if hasattr(astar_path, "xlist") and is_Astar_short:  # 如果A*规划成功，跳出循环
                break   # 退出while
            
            # 如果规划失败，尝试更后面的交接点
            handover_index += retreat_dist * 10
            if handover_index >= len(path_planned):
                break
            if handover_index >= retreat_dist * 20:
                break
        
        if not hasattr(astar_path, "xlist"):
            print("A*寻路失败，返回空路径")
            return path_planned, astar_path
        
        # 生成新的路径（从起点到交接点的A*路径 + 交接点之后的原路径）
        new_path = []
        
        # 将A*路径添加到新路径前面
        for j in range(1, len(astar_path.xlist)):
            new_path.append(
                [astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j], astar_path.directionlist[j]])

        # 添加交接点之后的原路径（需要确保不重复添加handover点）
        new_path.extend(path_planned[handover_index+1:])

        return new_path, astar_path

        # file_path = os.path.join(dir_algorithm, "outputs", "plot", "new_path.png")    # 调试用代码，画出路径
        # os.makedirs(os.path.dirname(file_path), exist_ok=True)
        # plt.figure(figsize=(6.2, 5), dpi=400)
        # x_coords = [point[0] for point in new_path]
        # y_coords = [point[1] for point in new_path]
        # plt.scatter(x_coords, y_coords, c='r', s=0.2)
        # plt.savefig(file_path)
        # print(f"路径规划结果已保存至{file_path}")
        # plt.close()


    def check_yaw_difference(self, path_planned_bak, start):
        # 获取第一个点的偏航角（弧度）并转换为角度
        first_point_yaw_deg = math.degrees(path_planned_bak[0][2])
        
        # 获取起点的偏航角（弧度）并转换为角度
        start_yaw_deg = math.degrees(start[2])
        
        # 计算角度差的绝对值（确保在0-180度范围内）
        angle_diff = abs((first_point_yaw_deg - start_yaw_deg + 180) % 360 - 180)
        
        print(f"初始偏航角差={angle_diff}度")
        return angle_diff


